#ifndef AXMAPCONTROL_H
#define AXMAPCONTROL_H

#include <QWidget>
#include <QThread>
#include <QtGui>
#include <QMutex>
#include <QMenu>
#include <QKeyEvent>
#include <QMouseEvent>
#include <QMessageBox>
#include <QMap>
#include <QCursor>
#include "ui_axmapcontrol.h"
#include <../QGLViewer/qglviewer.h>
#include <../QGLViewer/manipulatedCameraFrame.h>
#include <vector>
#include "scanmatcher/smmap.h"
#include "utils/point.h"
#include "vec.h"
#include <functional>
#include "constraint.h"

using namespace std;
using namespace GMapping;

#define  AX_DEFAULT 0
#define  AX_DRAW_DIRECTION 1
#define  AX_AUTOEXPLORER 3
#define  AX_Add_Path 2

class AxControlViewer;

typedef struct
{
	GLfloat x;
	GLfloat y;
} vector_t;

typedef struct
{
	GLfloat x;
	GLfloat y;
	GLfloat z;
} vector3_t;

typedef vector<vector_t> scan_data_t;
typedef vector<scan_data_t> scans_t;

const double Default_mm_per_pixel = 10.0;

typedef vector<vector_t> Points;
typedef vector<vector3_t> Point3ds;
typedef vector<Points> Points_group;

class AxMapControl :  public QGLViewer
{
	Q_OBJECT

public:
	AxMapControl(QWidget *parent = 0);
	~AxMapControl();
	void set_plot_data(Point3ds pts,long timestamp);
	void clear_plot_data();
	void save();
	void redraw(void);

	void drawMap(const QImage& img,GMapping::Point downleft,GMapping::Point upright);

	void setCurrentTool(int currentTool_);

	void setCurrentPose(double x,double y,double theta);

	void setCurrentOdometry(double x,double y,double theta);

	void setGoal(double x,double y,double theta);
	qglviewer::Vec getPlaningPose();
	struct pImpl;
	std::auto_ptr<pImpl> pimpl;
	int currentTool;

	std::function<void(qglviewer::Vec Pnt)> fun;
	std::function<void(QKeyEvent *e)> funKeyEvent;

	QMutex globel_mutex_;
protected:
	virtual void draw();
	virtual void init();
	virtual void mousePressEvent(QMouseEvent* e);
	virtual void keyPressEvent(QKeyEvent *e);
	virtual void postSelection(const QPoint& point);
	virtual void drawWithNames();
	virtual QString helpString() const;


private:
	Ui::AxMapControl ui;
	qglviewer::Vec orig, dir, selectedPoint;
	qglviewer::Vec Pnt;			

	qglviewer::ManipulatedFrame* frame;

	float ratio, u_max, v_max;
	QImage glImg;
	Point lowLeft;
	Point upRight;
};

#endif // AXMAPCONTROL_H